#include <stdlib.h>
#include <string.h>

#include "FreeRTOS.h"
#include "timers.h"
#include "soft_timer_task.h"

#include "led.h"
#include "utrasonic.h"

#define CHECK_TIMER_PERIOD_MS   (5000/portTICK_RATE_MS) /* 1s */
#define DONT_BLOCK              0

#define SOFT_LED_PIN            GPIO_Pin_14
#define SOFT_LED_GPIO_PORT      GPIOD
#define SOFT_LED_GPIO_CLK       RCC_AHB1Periph_GPIOD
static struct x_utrasonic_infos utrasound;
static struct x_gpio            trigger_pin;
static struct x_gpio            echo_pin;
static unsigned long
utrasound_read_callback(struct x_utrasonic *data)
{
    if (!data) {
        while(1){}
    }
    return 1;
}
static void
timer_callback_handler( xTimerHandle xTimer )
{
    /* do something here every 1s */
    //if( xTimerIsTimerActive( xTimer ) != pdFALSE ) {
    //    while(1){}
    //}
    toggle_led(SOFT_LED_GPIO_PORT, SOFT_LED_PIN);
    start_utrasonic_measure(&utrasound, utrasound_read_callback);
}

xRET_CODE_TYPE
timer_service_init(unsigned long xpriority)
{
    xTimerHandle xCheckTimer = NULL;

    init_led(SOFT_LED_GPIO_CLK, SOFT_LED_GPIO_PORT, SOFT_LED_PIN);
    /*
     * Utrasound test
     */
    trigger_pin.port = GPIO_TRIGGER_PORT;
    trigger_pin.pin = GPIO_TRIGGER_PIN;
    trigger_pin.rcc = RCC_TRIGGER_PORT;
    trigger_pin.is_input = 0;
    utrasound.trigger_pin = trigger_pin;
    utrasound.echo_pin = echo_pin;
    utrasound.temperature = 20;
    open_utrasonic_module(&utrasound);
    xCheckTimer = xTimerCreate( ( const signed char * ) "CheckTimer",/* A text name, purely to help debugging. */
                                ( CHECK_TIMER_PERIOD_MS ),		/* The timer period, in this case 3000ms (3s). */
                                pdTRUE,								/* This is an auto-reload timer, so xAutoReload is set to pdTRUE. */
                                ( void * ) 0,						/* The ID is not used, so can be set to anything. */
                                timer_callback_handler				/* The callback function that inspects the status of all the other tasks. */
                               );

    if( xCheckTimer != NULL ){
        xTimerStart( xCheckTimer, DONT_BLOCK );
        return(xSUCCESS);
    } else {
        return(xERROR);
    }
}
